#include "stdio.h"
#include "stdint.h"
#include "math.h"
#include "DevicesPID.h"


#define PID_ABS(x) (((x) > 0.0f) ? (x) : (0.0f - (x)))


void vPIDDeInit(PIDInfoType *ptypeHandle, float fkp, float fki, float fkd, float fErrorMax, float fIntegralMax, float fIntegralMin, float fExportValueMax, float fExportValueMin)
{
    ptypeHandle->exportValue    = 0.0f;
    ptypeHandle->errLast0       = 0.0f;
    ptypeHandle->integral       = 0.0f;
    ptypeHandle->kp             = fkp;
    ptypeHandle->ki             = fki;
    ptypeHandle->kd             = fkd;

    ptypeHandle->errMax         = fErrorMax;
    ptypeHandle->integralMax    = fIntegralMax;
    ptypeHandle->integralMin    = fIntegralMin;
    ptypeHandle->exportValueMax = fExportValueMax;
    ptypeHandle->exportValueMin = fExportValueMin;
}

/*
Return Value:   PID调节后的输出值
Parameters:     fSetValue: 设定值;
                fActualValue: 当前值;
Description:    位置型PID调节算法
*/
float fPIDLocation(PIDInfoType *ptypePID, float fSetValue, float fActualValue)
{
    float fIntegral = 0.0;

    ptypePID->err = fSetValue - fActualValue;

    /* 限制积分超调 */
    if(PID_ABS(ptypePID->err) < ptypePID->errMax)
        ptypePID->integral += ptypePID->err;
    else
        ptypePID->integral = 0.0f;

    fIntegral = ptypePID->ki * ptypePID->integral;
    /* 限定积分上、下限 */
    if(fIntegral > ptypePID->integralMax)
        ptypePID->integral = ptypePID->integralMax / ptypePID->ki;
    else if(fIntegral < ptypePID->integralMin)
        ptypePID->integral = ptypePID->integralMin / ptypePID->ki;

    /* PID 运算 */
    ptypePID->exportValue = ptypePID->kp * ptypePID->err
                          + ptypePID->ki * ptypePID->integral
                          + ptypePID->kd * (ptypePID->err - ptypePID->errLast0);

    ptypePID->errLast0 = ptypePID->err;

    /* 限定输出上、下限 */
    if(ptypePID->exportValue > ptypePID->exportValueMax)
        ptypePID->exportValue = ptypePID->exportValueMax;
    else if(ptypePID->exportValue < ptypePID->exportValueMin)
        ptypePID->exportValue = ptypePID->exportValueMin;

    return ptypePID->exportValue;
}
